#include "cppn.hh"
#include "node.hh"
#include "constants.hh"
#include <vector>
#include <cassert>
#include <iostream>
#include <algorithm>
#include <utility>
#include <map>
using namespace std;

namespace {
    constexpr int WEIGHT = 0;
    constexpr int LEO = 1;
}

namespace cppn {
    using evolution::Genome;
/*
    struct 
    CPPN::FastNetwork {
        //state of the network (output of neurons)
        vector<float> state;

        //weights in-order
        vector<float> weights;
        vector<size_t> inputs;

        //offset of weights, number of weights, activation_fun bias
        static const int OFFSET = 0, SIZE = 1, TYPE = 2, BIAS = 3;
        vector<tuple<size_t, size_t, evolution::NodeType, float>> controls; 
        
        //construction helper: genetic neuron id to offset in control vector
        std::vector<std::pair<Node*, size_t>> in_order;
        int ids = 0;
        
        void add_neuron (Node* neuron) {
            in_order.push_back(make_pair(neuron, ids++));
        }

        void create_network () {
            state.reserve(in_order.size());
            controls.reserve(in_order.size());

            size_t current_offset = 0;
            for (auto const& n: in_order) {        
                Node const* neuron = n.first;
                size_t n_connections = neuron->get_inputs().size();
                
                for (auto const& connection: neuron->get_inputs()) {
                    size_t other_id = id_of(connection.first);
                    
                    float weight = connection.second;
          
                    weights.push_back(weight);
                    inputs.push_back(other_id);
                }
           
                state.push_back(0);
                controls.push_back(make_tuple(current_offset, n_connections, 
                                              neuron->get_type(),
                                              neuron->get_bias()));
                current_offset += n_connections;
            }
        }

        size_t id_of (Node* n) {
            for (auto const& iter: in_order) {
                if (iter.first == n) return iter.second;
            }
            assert(false);
        }

        void step () {
            for (size_t neuron = 0; neuron < controls.size(); ++neuron) {
                auto const& ctrl = controls[neuron];
                size_t const offset = get<OFFSET>(ctrl);
                size_t const size = get<SIZE>(ctrl);
                evolution::NodeType const type = get<TYPE>(ctrl);
                float const bias = get<BIAS>(ctrl);

                if (neuron >= INPUT_NODES) {
                    float input = 0;
                    for (size_t s = offset; s < offset+size; ++s) {
                        input += state[inputs[s]] * weights[s];
                    }
                    float out = Node::activation(type, input, bias);
                    if (out > 1) out = 1;
                    else if (out < -1) out = -1;
                    else if (isnan(out)) out = 0;            
                    state[neuron] = out;
                } 
            }
        }

        void set_input (float x1, float y1, float x2, float y2, float d) {
            state[0] = x1;
            state[1] = y1;
            state[2] = x2;
            state[3] = y2;
            state[4] = d;
            state[5] = 1; //bias
        }

        float get_weight () const {
            return state[state.size()-2];
        }

        float get_leo () const {
            return state[state.size()-1];
        }

        void reset () {
            fill(state.begin(), state.end(), 0.0f);
        }

        friend ostream& operator<<(ostream& out, FastNetwork const& net) {
            int n = 0;
            for (auto const& s: net.state) {
                out << n++ << ": " << s << endl;
            }
            return out;
        }
    };
*/

    struct CPPN::Builder {
        vector<Node*> neurons;

        Node* get_neuron(int id) {
            auto iterator = find_if(neurons.begin(), neurons.end(), [=](Node* n) { return n->get_id() == id; });
            return iterator == neurons.end() ? nullptr : *iterator;
        }

        
        void set_inputs (Node* x1, Node* y1, 
                         Node* x2, Node* y2,
                         Node* dist, Node* bias) {
            neurons.push_back(x1);
            neurons.push_back(y1);
            neurons.push_back(x2);
            neurons.push_back(y2);
            neurons.push_back(dist);
            neurons.push_back(bias);
        }

        void set_outputs (Node* weight, Node* leo) {
            neurons.push_back(weight);
            neurons.push_back(leo);
        }

        void add (Node* n) {
            neurons.push_back(n);
        }

        ~Builder() {
            for (Node* node: neurons) delete node;
        }
    };


    CPPN::CPPN (shared_ptr<Genome> genome) 
    {
        unique_ptr<Builder> builder = unique_ptr<Builder>(new Builder);
        assert(genome->nodes() >= 8);

        auto iterator = genome->get_node_genes().begin ();
        Node* x1 = new Node(*iterator++);
        Node* y1 = new Node(*iterator++);
        Node* x2 = new Node(*iterator++);
        Node* y2 = new Node(*iterator++);
        Node* dist = new Node(*iterator++);
        Node* bias = new Node(*iterator++);
        builder->set_inputs(x1, y1, x2, y2, dist, bias);


        for (int id = TOTAL_INPUT_NODES; id < genome->nodes()-OUTPUT_NODES; ++id) {
            builder->add(new Node(*iterator++));
        }

        Node* weight = new Node(*iterator++);
        Node* leo = new Node(*iterator++);

        builder->set_outputs(weight, leo);

        assert(iterator == genome->get_node_genes().end());

        int n_conns = 0;
        for (auto connection: genome->get_connection_genes()) {
            if (connection.get_enabled() && connection.get_weight() != 0.0f) {
                n_conns++; 
                builder->get_neuron(connection.get_source())->connect_to 
                    (builder->get_neuron(connection.get_target()), connection.get_weight());
            }
        }

        ann::NodeCollection collection;
        for (Node* n: builder->neurons) collection.add_node(n);

        net = new ann::Network{INPUT_NODES, BIAS_NODES, OUTPUT_NODES, &Node::activation, collection};
    }

    CPPN::~CPPN() {
        delete net;
    }

    void
    CPPN::set_input (float x1, float y1, float x2, float y2) {
        float d2 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
        net->set_input({x1, y1, x2, y2, sqrt(d2)});
        net->step();
    }



    void
    CPPN::reset () {
        net->reset();
    }

    bool
    CPPN::get_leo() {
        return net->get(LEO) > 0.2f;
    }

    float
    CPPN::get_weight() {
        return net->get(WEIGHT); 
    }

    ostream& operator<< (ostream& out, CPPN const& cppn) {
        return out ; //<< *cppn.net;
    }

}





















